//#include <MelodyUtils.h>
#include <pitches.h>

#include <Servo.h>
#include <Potentiometer.h>
#include <NewPing.h>

/// STATE MACHINE VARIABLES ///

// Table/Servo
boolean isSystemInitialized = false;

boolean isTableClosed = false;
// If table is not closed, it might be either moving up, down, stopping in between or is open. 
boolean isTableMovingUp = false;
boolean isTableMovingDown = false;
boolean isTableHalted = false;
boolean isTableOpen = false;

// Servo Switch is initally pressed to open the table.
boolean isTableSwitchOn = true;

// Crash sensor for the table;
boolean isCrashDetected = false;

// Light Switch is initially on (lights on on startup).
boolean isLightSwitchOn = true;

/// END: STATE MACHINE VARIABLES ///

// Ultrasonic sensor settings
// max sensor distance in cm
#define MAX_DISTANCE 200
int ultraSonicTriggerPin = 12;
int ultraSonicReceiverPin = A2;
int currentSensedDistance = 0; 
// Till this value is not false, the table servos must NOT move.
NewPing sonar(ultraSonicTriggerPin, ultraSonicReceiverPin, MAX_DISTANCE);

// Servo switch pins
int tableSwitchPin = 3; // is physical pin 1
int topTableSwitchPin = 7;
int bottomTableSwitchPin = 8;

// Main light switch
int lightSwitchPin = 0; //phsiscally it's pin 3

// Gate pin for the MOSFET light dimmers
int upperLightMosfetDimmerPin = 11;
int sideLightMosfetDimmerPin = A3;
Potentiometer potiUpperLightPin = Potentiometer(A0); //a Potentiometer at analog in 2
Potentiometer potiSideLightPin = Potentiometer(A1); //a Potentiometer at analog in 1

Servo tableServoLeft;
Servo tableServoRight;
// Only pin 6 and 5 seem to work for servos.
int servoLeftPin = 9;
int servoRightPin = 10;
int servoMosfetSwitchPin = 4;
int servoAngleDeviation = 0;
int servoAngleDeviationRight = 0;
int servoAngleDeviationLeft = 1;

// Solenoid pin
int solenoidTableLockPin = A5; 
// Crash sensor interrupt pin;
int crashSensorPin = 2;



int melody[] = {NOTE_C4, NOTE_G3,NOTE_G3, NOTE_A3, NOTE_G3,0, NOTE_B3, NOTE_C4};
int noteDurations[] = {4, 8, 8, 4,4,4,4,4 };

// The setup routine runs once when you press reset:
void setup() {
  Serial.begin(300);
  Serial.println("WorkbenchControl V1");
  
  //playMelodyStarWars();
  //MelodyUtils mel(A4);
  //mel.Glis(NOTE_C3, NOTE_C4, 5);
  delay(1000);
  
  
//  pinMode(ultraSonicTriggerPin, OUTPUT);
//  pinMode(ultraSonicReceiverPin, INPUT);
  
  tableServoLeft.attach(servoLeftPin);
  tableServoRight.attach(servoRightPin);
  
  // Set servo switches.
  attachInterrupt(tableSwitchPin, switchTable, RISING);
  attachInterrupt(crashSensorPin, alertTransition, CHANGE);
  pinMode(topTableSwitchPin, INPUT);
  pinMode(bottomTableSwitchPin, INPUT);
  pinMode(solenoidTableLockPin, OUTPUT);
  
  // Set the light switch to digital input
  pinMode(lightSwitchPin, INPUT);
  attachInterrupt(lightSwitchPin, switchLights, LOW);
  
  initalise();
  
  // Postpone startup
  delay(3000);
}

void initalise(){  
  /// INITIALISE THE SYSTEM ///
  // Motors/table: 
  if(isTableClosedNow() == true && isTableSwitchOn == true){
    // now, move servos 90° downwards    
    if(openTable() == true){
      Serial.println("Succesully opened table.");
      isSystemInitialized = true;
    }
    else{  
      Serial.println("Failed to open table.");  
      isSystemInitialized = true;
    }    
  } else{   
    Serial.println("Could not open table, because upper table switch is not pushed.");
    isSystemInitialized = false;    
  }  
}

void playSound(){
 for (int thisNote = 0; thisNote < 8; thisNote++) {
    // to calculate the note duration, take one second
    // divided by the note type.
    //e.g. quarter note = 1000 / 4, eighth note = 1000/8, etc.
    int noteDuration = 1000/noteDurations[thisNote];
    tone(A4, melody[thisNote],noteDuration);

    // to distinguish the notes, set a minimum time between them.
    // the note's duration + 30% seems to work well:
    int pauseBetweenNotes = noteDuration * 1.30;
    delay(pauseBetweenNotes);
    // stop the tone playing:
    noTone(8);
  } 
}

// the loop routine runs over and over again forever:
void loop() {
  if(isSystemInitialized){  
      
    // Check for the motor switches
//    Serial.println(digitalRead(topTableSwitchPin));
//    Serial.println(digitalRead(bottomTableSwitchPin));
    
    // If table switch is pressed then move, either close or open the table.
    if(isTableSwitchOn == true){            
      // Flip the switch event, in order to allow interrupts, when moving.
      isTableSwitchOn = false;
      
      if(isTableClosedNow() == true){
          if(openTable() == false)
            Serial.println("Failed to open table.");  
       }        
       else if(isTableOpenNow() == true){
         // Check if there is something on the table.
          if(isTableEmpty() == true){
            if(closeTable() == false)
              Serial.println("Failed to close table.");
         } else{
           Serial.println("The table is not empty. Clean up before closing the table.");
         }  
       }
       isTableSwitchOn = false;      
    }
    
    // The main light switch (turning all lights on or off).   
    if(isLightSwitchOn){
      // Dimming the upper lights.
      int potiUpperLightValue = potiUpperLightPin.getValue();      
      int relPotiUpperLightValue = map(potiUpperLightValue, 0, 1023, 0, 255); 
      analogWrite(upperLightMosfetDimmerPin, relPotiUpperLightValue); 
      // Dimming the side lights.
      int potiSideLightValue = potiSideLightPin.getValue();
      int relPotiSideLightValue = map(potiSideLightValue, 0, 1023, 0, 255);
//      Serial.print(relPotiUpperLightValue);  
//      Serial.print(" :: ");
      //Serial.println(relPotiSideLightValue);
      analogWrite(sideLightMosfetDimmerPin, relPotiSideLightValue);    
    }else{
      analogWrite(upperLightMosfetDimmerPin, 0);
      analogWrite(sideLightMosfetDimmerPin, 0);
    }  
  }
  else{
    Serial.println("System was not initalized. Try again in 3 seconds.");
    delay(3000);
    // init again
    initalise();  
  }
  
  // Set the latch light switch to false, because interupt will set it to true anyways.
  isLightSwitchOn = false;
  delay(25);
}

//TODO
// Check if anythMing is on the table
boolean isTableEmpty(){
  int currentSensedDistance = sonar.ping();
  // TODO Mit Servo Tisch abmessen!
  //if(currentSensedDistance < 50)
  //  setTablePopulated();
//  Serial.print("Ping: ");
//  Serial.print(sonar.convert_cm(currentSensedDistance));
//  Serial.println("cm");
  return true;
}

boolean closeTable(){
    if(isTableOpenNow() == true){
      Serial.println("Closing Table. Starting MOSFET.");
      // Send more than 2 Volt to trigger the servo MOSFET. 
      digitalWrite(servoMosfetSwitchPin, HIGH);
      int pos = 90;
      for(pos = (90+servoAngleDeviation); pos >= (0 + servoAngleDeviation); pos -= 1) // goes from 90 degrees to 0 degrees 
      {                                  // in steps of 1 degree 
        // Check if there is an alert
        if(isTableToHalt() == true){
          Serial.println("Interuped table closing.");
          return false;
        }
        // Instruct servo to go to position in variable 'pos'
        tableServoRight.write(pos+7);            
        tableServoLeft.write(90 - pos-1);    
        
        if(pos <= servoAngleDeviation+10){
          digitalWrite(solenoidTableLockPin, HIGH);
          Serial.println("Opening Table Lock");
        }else{
          digitalWrite(solenoidTableLockPin, LOW);
        }
        delay(50);                        
      }
      // No set the state variable
      setTableClosed();    
      // Arrest the table lock. Release the solenoid.
      digitalWrite(solenoidTableLockPin, LOW);
      Serial.println("Closing table lock");
      
      // Turn off the Servo.
      Serial.println("Table is fully closed. Shutting down MOSFET.");
      digitalWrite(servoMosfetSwitchPin,LOW); 
      return true;
    }else{
      Serial.println("Table is already closed");
      return true;
    }
}

boolean openTable(){
  if(isTableClosedNow() == true){
    Serial.println("Opening Table. Starting MOSFET.");
    // Send more than 2 Volt to trigger the servo MOSFET. 
    digitalWrite(servoMosfetSwitchPin, HIGH);
    int pos = 0;
    for(pos = (0+servoAngleDeviation); pos <= (90 + servoAngleDeviation); pos += 1) // goes from 0 degrees to 90 degrees 
    { 
      // Open the table lock
      if(pos < 10){
        digitalWrite(solenoidTableLockPin, HIGH);
        Serial.println("Opening table lock");
      }
      else{
        digitalWrite(solenoidTableLockPin, LOW);
      }
      
      // in steps of 1 degree 
      // Check if there is an alert.
      if(isTableToHalt() == true){
        Serial.println("Interuped table opening.");
        return false;
      }
      // Instruct servo to go to position in variable 'pos'
      tableServoRight.write(pos+7);
      tableServoLeft.write(90 - pos-1); 
      
      delay(50);                       
    }
    // No set the state variable
    setTableOpen();    
    // Kick in the solenoid to block the table.    
    // Turn off the Motor
    Serial.println("Table is fully opened. Shutting down MOSFET.");
    digitalWrite(servoMosfetSwitchPin,LOW); 
    return true;
  } else {
    Serial.println("Table is already closed");
    return true;
  }
}

boolean isTableToHalt(){
    if(isCrashDetected == true){      
      Serial.println("Table disctruction detected.");
      return true;
    }
    else if(isTableSwitchOn == true){
      Serial.println("Table distruction detected: table Switch has been pressed.");
      return true;
    }
    else
      return false;
}

boolean isTableClosedNow(){
  if(digitalRead(topTableSwitchPin) == 1){
    isTableClosed = true;
    // Set the others table states.
    isTableOpen = false;
    isTableMovingUp = false;
    isTableMovingDown = false;
    isTableHalted = false;
  }
  else{
    isTableClosed = false;
  }
  return isTableClosed;
}

boolean isTableOpenNow(){
  if(digitalRead(bottomTableSwitchPin) == 1){
    isTableOpen = true;
    // Set the others table states.
    isTableClosed = false;
    isTableMovingUp = false;
    isTableMovingDown = false;
    isTableHalted = false;
  }
  else{
    isTableOpen = false;
  }
  return isTableOpen;  
}

void setTableOpen(){
    isTableOpen = true;
    // Set the others table states.
    isTableClosed = false;
    isTableMovingUp = false;
    isTableMovingDown = false;
    isTableHalted = false;
}

void setTableClosed(){
    isTableClosed = true;
    // Set the others table states.
    isTableOpen = false;
    isTableMovingUp = false;
    isTableMovingDown = false;
    isTableHalted = false;
}

// Interrupt method to pull up the table
void switchTable()
{
  //Set the crash flag to false, in order to start over again.
  isCrashDetected = false;
  
  static unsigned long last_interrupt_time = 0;
  unsigned long interrupt_time = millis();
  // Avoid button bouncing.
  // If interrupts come faster than 500ms, assume it's a bounce and ignore
  if (interrupt_time - last_interrupt_time > 500){
    isTableSwitchOn = true;        
    Serial.println("Table switch is now flaged. Instructing to move the table towards the other direction.");    
  }
  last_interrupt_time = interrupt_time;    
}

// Halts the table when moving.
void alertTransition(){
  isCrashDetected = true;
  if(isTableClosedNow() == false && isTableOpenNow() == false){
    //playAlertMelody();
    //MelodyUtils mel(A4);
    //mel.Trem(NOTE_C3, 1000, 30);
    Serial.println("CRASH DETECTED");
  }
}

// This method is always triggered, when latch button is released.
void switchLights()
{
//  Serial.print("set it to light on: ");
//  Serial.println(digitalRead(lightSwitchPin));
  isLightSwitchOn = true;
  //isLightSwitchOn = !isLightSwitchOn;
//  static unsigned long last_interrupt_time = 0;
//  unsigned long interrupt_time = millis();
  
//  // Avoid button bouncing.
//  // If interrupts come faster than 500ms, assume it's a bounce and ignore
//  if (interrupt_time - last_interrupt_time > 500) 
//  {
//    isLightSwitchOn = !isLightSwitchOn;  
//    Serial.print("lightSwitchPin: ");
//    Serial.println(digitalRead(lightSwitchPin));
//    if(isLightSwitchOn)
//      Serial.println("Light switch is turned on");
//    else
//      Serial.println("Light switch is turned off");
//  }
//  last_interrupt_time = interrupt_time;    
}

void playAlertMelody(){
  tone(A4, NOTE_G2, 600);
  delay(600);
 
  tone(A4, NOTE_G2, 600);
  delay(600);
}

void playMelodyStarWars(){
  tone(A4, NOTE_G2, 600);
  delay(600);
 
  tone(A4, NOTE_G2, 600);
  delay(600);
 
  tone(A4, NOTE_G2, 600);
  delay(600);
 
  tone(A4, NOTE_DS2, 500);
  delay(500);
 
  tone(A4, NOTE_AS2, 150);
  delay(150);
 
  tone(A4, NOTE_G2, 600);
  delay(600);
 
  tone(A4, NOTE_DS2, 500);
  delay(500);
 
  tone(A4, NOTE_AS2, 150);
  delay(150);
 
  tone(A4, NOTE_G2, 1200);
  delay(1200);
 
  tone(A4, NOTE_D3, 600);
  delay(600);
 
  tone(A4, NOTE_D3, 600);
  delay(600);
 
  tone(A4, NOTE_D3, 600);
  delay(600);
 
  tone(A4, NOTE_DS3, 500);
  delay(500);
 
  tone(A4, NOTE_C3, 150);
  delay(150);
 
  tone(A4, NOTE_FS2, 600);
  delay(600);
 
  tone(A4, NOTE_D2, 500);
  delay(500);
 
  tone(A4, NOTE_AS2, 150);
  delay(150);
 
  tone(A4, NOTE_G2, 1200);
  delay(1200);
 
  noTone(A4);
}
